% Standard Position Joint Angles

neutral = [0.0;-0.5416;0.0;-2.8859;-0.0864;2.3600;0.7462];
gripper_testing = [0.0; 0.3507;0.0;-1.9392;0.0800;2.3512;0.8057];
look_left = [1.5700; 0.0;0.0;-1.5700;0.0;1.5700;0.7850];
look_right = [-1.5700; 0.0;0.0;-1.6700;0.0;1.5700;0.7850];
grasping_test = [0.0835; 0.7124; -0.0602;-1.8567;0.0698;2.6190;0.7714];

% Standard Position O_T_EE Matrices

Middle = createTrafo(0.4,0,0.2);
Low = createTrafo(0.4,0,0.05);
Left = createTrafo(0.4,-0.15,0.2);
Right = createTrafo(0.4,0.15,0.2);

Box1 = createTrafo(0.4,0,0.2);
Box2 = createTrafo(0.4,0.3,0.2);
Box3 = createTrafo(0.4,0.4,0.2);

ip = '172.16.0.2';
speed = 0.2;
mdl_panda;